#include "CMeCamera.h"



CMeCamera::CMeCamera()
{
}


CMeCamera::~CMeCamera()
{
}


bool CMeCamera::EnumDevices(s_DevicesInfo& sInfo)
{
	std::vector<mmind::eye::CameraInfo> deviceInfoList = mmind::eye::Camera::discoverCameras();
	
	sInfo.iNum = deviceInfoList.size();

	if (deviceInfoList.empty()) 
	{
		return false;
	}

	for (int i = 0; i < deviceInfoList.size(); i++) 
	{

		sInfo.sCameraInfo[i].SN = deviceInfoList[i].serialNumber;
		sInfo.sCameraInfo[i].sModelName = deviceInfoList[i].model;
		sInfo.sCameraInfo[i].sDeviceUserID = deviceInfoList[i].deviceName;
		sInfo.sCameraInfo[i].IpAddr= deviceInfoList[i].ipAddress;                  //IP
	}
    return true;
}


bool CMeCamera::IsOpen()
{
	return m_bOpen;
}

//bool CMeCamera::IsStart()
//{
//	return m_bStartGrap;
//}


bool CMeCamera::Open(std::string ip)
{

	if (ip == "")
	{
		return false;
	}


	//发现设备
	std::vector<mmind::eye::CameraInfo> deviceInfoList = mmind::eye::Camera::discoverCameras();
	if (deviceInfoList.empty())
	{
		return false;
	}

	if (m_bOpen)
	{
		return false;
	}

	// 选择对应的设备进行初始化--直接以ip打开


	// 打开设备 
	mmind::eye::ErrorStatus status;
	status = m_camera.connect(ip);
	if (!status.isOK())
	{
		showError(status);
		return false;
	}


	m_bOpen = true;
	//m_bStartGrap = false;


	return true;


}


bool CMeCamera::SoftwareTrigger(s_Image3dS& image3dS, unsigned int iTimeout)
{


    try
    {
        //触发
        mmind::eye::ErrorStatus status;

        mmind::eye::Frame2DAnd3D frame2DAnd3D;
        status = m_camera.capture2DAnd3D(frame2DAnd3D);
        if (!status.isOK())
        {

            return false;
        }

        ///////////////////////////////////////////////////////////////////////////////////////////
        //图像提取到halcon格式
        ///////////////////////////////////////////////////////////////////////////////////////////

        ////2D图--灰度
        //HObject hImageGray;

        //mmind::eye::Gray* pDataGray = frame2DAnd3D.frame2D().getGrayScaleImage().data();
        //uint8_t* pGrayBytes = reinterpret_cast<uint8_t*>(pDataGray);

        //int iWidth = (int)frame2DAnd3D.frame2D().getGrayScaleImage().width();
        //int iHeight = (int)frame2DAnd3D.frame2D().getGrayScaleImage().height();

        //   GenImage1(&hImageGray, "byte", (HTuple)iWidth, (HTuple)iHeight, (Hlong)pGrayBytes);

        ////WriteImage(hImageGray, "bmp", 0, "hImageGray");

        //std::cout << "hImageGray finish" << std::endl;


        ////2D图--彩色

        //mmind::eye::Color2DImage  colorImage = frame2DAnd3D.frame2D().getColorImage();

        //HObject hImageColor;

        //int iWidthColor = colorImage.width();
        //int iHeightColor = colorImage.height();
        //int arrLength = iWidthColor * iHeightColor;

        //uint8_t* redArr = new uint8_t[arrLength];
        //uint8_t* greenArr = new uint8_t[arrLength];
        //uint8_t* blueArr = new uint8_t[arrLength];

        //for (int i = 0; i < arrLength; i++)
        //{
        // redArr[i] = colorImage[i].r;
        // greenArr[i] = colorImage[i].g;
        // blueArr[i] = colorImage[i].b;
        //}


        //try
        //{
        // GenImage3(&hImageColor, "byte", (HTuple)iWidthColor, (HTuple)iHeightColor, (Hlong)redArr, (Hlong)greenArr, (Hlong)blueArr);
        //}
        //catch (HalconCpp::HException& ex)
        //{
        // delete[] redArr;
        // delete[] greenArr;
        // delete[] blueArr;
        // redArr = nullptr;
        // greenArr = nullptr;
        // blueArr = nullptr;
        //}




        //WriteImage(hImageColor, "bmp", 0, "hImageColor");

        //std::cout << "hImageColor finish" << std::endl;



        //3D Map
        mmind::eye::TexturedPointCloudWithNormals  xyznxnynz = frame2DAnd3D.getTexturedPointCloudWithNormals();

        // 获取宽高
        int height = xyznxnynz.height();    // 行数（高）
        int width = xyznxnynz.width();    // 列数（宽）
        int  totalPoints = width * height;

        // 在堆上分配数组
        float* ArrayX = new float[totalPoints];
        float* ArrayY = new float[totalPoints];
        float* ArrayZ = new float[totalPoints];
        float* ArrayNX = new float[totalPoints];
        float* ArrayNY = new float[totalPoints];
        float* ArrayNZ = new float[totalPoints];

        uint8_t* redArr = new uint8_t[totalPoints];
        uint8_t* greenArr = new uint8_t[totalPoints];
        uint8_t* blueArr = new uint8_t[totalPoints];



        // 获取点云数据指针
        mmind::eye::PointXYZBGRWithNormals* rawData = xyznxnynz.data();

// ? 并行提取（OpenMP）
#pragma omp parallel for if(totalPoints > 50000)
        for (int idx = 0; idx < totalPoints; ++idx)
        {
            const auto& p = rawData[idx];

            if (std::isnan(p.colorPoint.x))
            {
                //无效数据先补0 后续算法再 去除
                //std::cout << "x 是 NaN" << std::endl;


                ArrayX[idx] = 0.0;
                ArrayY[idx] = 0.0;
                ArrayZ[idx] = 0.0;

                ArrayNX[idx] = 0.0;
                ArrayNY[idx] = 0.0;
                ArrayNZ[idx] = 0.0;

                redArr[idx] = 0;
                greenArr[idx] = 0;
                blueArr[idx] = 0;
            }
            else
            {
                ArrayX[idx] = p.colorPoint.x;
                ArrayY[idx] = p.colorPoint.y;
                ArrayZ[idx] = p.colorPoint.z;

                ArrayNX[idx] = p.normal.x;
                ArrayNY[idx] = p.normal.y;
                ArrayNZ[idx] = p.normal.z;

                redArr[idx] = p.colorPoint.r;
                greenArr[idx] = p.colorPoint.g;
                blueArr[idx] = p.colorPoint.b;

            }

        }



        // 转换成halcon格式map图像
        HObject ho_X, ho_Y, ho_Z, ho_NX, ho_NY, ho_NZ;
        HObject hImageColor;
        HObject hImageGray;


        try
        {
            GenImage1(&ho_X, "real", width, height, (Hlong)ArrayX);
            GenImage1(&ho_Y, "real", width, height, (Hlong)ArrayY);
            GenImage1(&ho_Z, "real", width, height, (Hlong)ArrayZ);

            GenImage1(&ho_NX, "real", width, height, (Hlong)ArrayNX);
            GenImage1(&ho_NY, "real", width, height, (Hlong)ArrayNY);
            GenImage1(&ho_NZ, "real", width, height, (Hlong)ArrayNZ);

            GenImage3(&hImageColor, "byte", width, height, (Hlong)redArr, (Hlong)greenArr, (Hlong)blueArr);

            Rgb1ToGray(hImageColor, &hImageGray);



        }
        catch (HalconCpp::HException& ex)
        {

            delete[] ArrayX;
            delete[] ArrayY;
            delete[] ArrayZ;
            delete[] ArrayNX;
            delete[] ArrayNY;
            delete[] ArrayNZ;

            delete[] redArr;
            delete[] greenArr;
            delete[] blueArr;


            ArrayX = nullptr;
            ArrayY = nullptr;
            ArrayZ = nullptr;
            ArrayNX = nullptr;
            ArrayNY = nullptr;
            ArrayNZ = nullptr;

            redArr = nullptr;
            greenArr = nullptr;
            blueArr = nullptr;
        }





        //去除无效数据
        HObject ho_Region;
        Threshold(ho_Z, &ho_Region, 1, 99999);
        ReduceDomain(ho_X, ho_Region, &ho_X);
        ReduceDomain(ho_Y, ho_Region, &ho_Y);
        ReduceDomain(ho_Z, ho_Region, &ho_Z);

        ReduceDomain(ho_NX, ho_Region, &ho_NX);
        ReduceDomain(ho_NY, ho_Region, &ho_NY);
        ReduceDomain(ho_NZ, ho_Region, &ho_NZ);




        std::cout << "hImage3d finish" << std::endl;

        //WriteImage(ho_X, "tiff", 0, "ho_X");
        //WriteImage(ho_Y, "tiff", 0, "ho_Y");
        //WriteImage(ho_Z, "tiff", 0, "ho_Z");

        //WriteImage(ho_NX, "tiff", 0, "ho_NX");
        //WriteImage(ho_NY, "tiff", 0, "ho_NY");
        //WriteImage(ho_NZ, "tiff", 0, "ho_NZ");


        //添加到3ds

        m_iFrameID++;

        //拷贝原始信息
        s_Image3dS As;
        As.Gray = hImageGray.Clone();
        As.Color = hImageColor.Clone();
        As.X = ho_X.Clone();
        As.Y = ho_Y.Clone();
        As.Z = ho_Z.Clone();

        As.NX = ho_NX.Clone();
        As.NY = ho_NY.Clone();
        As.NZ = ho_NZ.Clone();

        As.ID = m_iFrameID;

        image3dS = As;

    }
    catch (HalconCpp::HException& ex)
    {

        return false;

    }


    return true;


}

void CMeCamera::Close()
{

	m_camera.disconnect();


	m_bOpen = false;

	//m_bStartGrap = false;
	m_iFrameID = -1;


}




